# Untitled - By: 64386 - 周六 8月 5 2023

import sensor, image, time, utime
import lcd, json
from Maix import GPIO
from fpioa_manager import fm
from machine import Timer, PWM, UART, Timer
import binascii
import math
#from board import board_info

lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.GRAYSCALE) # 灰度更快(160x120 max on OpenMV-M7)
sensor.set_framesize(sensor.QQVGA)#160 * 120
sensor.skip_frames(time = 2000)

clock = time.clock()

tim1 = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim2 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
S2 = PWM(tim1, freq=50, duty=0, pin=15)
S1 = PWM(tim2, freq=50, duty=0, pin=17)   #x
ratio = 6  #实际距离与像素距离之比
i_x = 0
i_y = 60
def Servo_x(servo,angle):
    #S2.duty((angle+90)/180*10+2.5)
    S1.duty((angle+90)/180*10+2.5)

def Servo_y(servo,angle):
    S2.duty((angle+90)/180*10+2.5)
   # S1.duty((angle+90)/180*10+2.5)
Servo_x(S1,0)
Servo_y(S2,-34.5)

flag = 1
fm.register(30,fm.fpioa.GPIOHS14)
fm.register(31,fm.fpioa.GPIOHS15)
fm.register(32,fm.fpioa.GPIOHS16)
fm.register(33,fm.fpioa.GPIOHS17)
fm.register(34,fm.fpioa.GPIOHS18)
fm.register(35,fm.fpioa.GPIOHS19)
#上拉按下为0 下拉按下为1
H1=GPIO(GPIO.GPIOHS15,GPIO.OUT)
H2=GPIO(GPIO.GPIOHS14,GPIO.OUT)
L1=GPIO(GPIO.GPIOHS16,GPIO.PULL_UP)
L2=GPIO(GPIO.GPIOHS17,GPIO.PULL_UP)
L3=GPIO(GPIO.GPIOHS18,GPIO.PULL_UP)
L4=GPIO(GPIO.GPIOHS19,GPIO.PULL_UP)
H1.value(1)
H2.value(1)
#iii = 1
#flag = 0
#flag1 = 0
#flag2 = 0
#d1 = []
#d2 = []
#d3 = []
#d4 = []
i_x = 0
i_y = 60
#zb_x = []
#zb_y = []
#jiao_x = []
#jiao_y = []
xunji = -1
iiii = 0
while(True):
    d = 0
    sensor.set_hmirror(1)
    clock.tick()
    img = sensor.snapshot()


    if xunji == 1:
        lcd.draw_string(100, 100, "11111", lcd.RED)
        while flag < 1:
            for r in img.find_rects(threshold = 30000):
                i = r.corners()
                print(i)
                for p in r.corners(): img.draw_circle(p[0], p[1], 5, color = (0, 255, 0))  #在四个角上画圆

                d1.append(i[3])
                d2.append(i[2])
                d3.append(i[1])
                d4.append(i[0])
                flag = flag + 1
                #print(flag)
                #print(d1)
                time.sleep(3)
                d1x = d1[0][0] + 3
                d2x = d2[0][0] - 3
                d3x = d3[0][0] - 3
                d4x = d4[0][0] + 3
                d1y = d1[0][1] + 3
                d2y = d2[0][1] + 3
                d3y = d3[0][1] - 0
                d4y = d4[0][1] - 0
                #zb_x.append(d1[0][0])
                #zb_x.append(d2[0][0])
                #zb_x.append(d3[0][0])
                #zb_x.append(d4[0][0])
                zb_x.append(d1x)
                zb_x.append(d2x)
                zb_x.append(d3x)
                zb_x.append(d4x)
                zb_y.append(d1y)
                zb_y.append(d2y)
                zb_y.append(d3y)
                zb_y.append(d4y)
                #zb_y.append(d4y)
                #print(zb_x)
                #print(zb_y)
                #lcd.display(img)
        while flag1 < 4 :
            #lcd.display(img)
            x = zb_x[flag1]  #从左上角开始，顺时针数第[flag1]个点的X坐标
            y = zb_y[flag1]  #从左上角开始，顺时针数第[flag1]个点的Y坐标
            #xx = -0.2422*x + 19.816
            #yy = 0.2086*y - 47.162
            xx = -0.2447*x + 19.271   #X方向角度 k = -0.2447 b = 19.271 #右上左下
            yy = 0.2334*y - 47.567    #Y方向角度 k = 0.2334 b = -47.567
            #xx = -0.2476*x + 19.335
            #yy = 0.235*y - 47.928

            jiao_x.append(xx)
            jiao_y.append(yy)
            print(jiao_x)
            flag1 = flag1 + 1
        Servo_x(S1,jiao_x[0])   #输入舵机第一个点的X方向角度

        Servo_y(S2,(jiao_y[0])) #输入舵机第一个点的Y方向角度
        utime.sleep_ms(1000)
        while flag2 < 4 :
            while iii < 301:
                flag3 = flag2 + 1
                if flag3 == 4:
                    flag3 = 0
                bujin_x = jiao_x[flag2] + (((jiao_x[flag3])-(jiao_x[flag2]))/300)*iii  #X的角度步进量

                bujin_y = jiao_y[flag2] + (((jiao_y[flag3])-(jiao_y[flag2]))/300)*iii  #Y的角度步进量
                Servo_x(S1,bujin_x)
                utime.sleep_ms(2)
                Servo_y(S2,bujin_y)   #每2ms移动一点角度
                #print(bujin_x)
                #print(bujin_y)
                iii = iii + 1
                utime.sleep_ms(2)
                #lcd.display(img)
            utime.sleep_ms(500)
            #x1 = jiao_x[flag2]
            #y1 = jiao_y[flag2] - 45

            flag2 = flag2 + 1
            #Servo_x(S1,x1)
            #Servo_y(S2,y1)
            #utime.sleep_ms(1000)
            #time.sleep(1)
                #Servo(S2,jiaoB)
            iii = 1
            #flag2 = flag2 + 1

    if xunji == -1:
        lcd.draw_string(100, 100, "00000", lcd.RED, lcd.BLACK)

    H1.value(0)
    if(L1.value()==0):  #确定原点
        xunji = -1*xunji
        print("开始")
        iii = 1
        flag = 0
        flag1 = 0
        flag2 = 0
        d1 = []
        d2 = []
        d3 = []
        d4 = []
        zb_x = []
        zb_y = []
        jiao_x = []
        jiao_y = []
        utime.sleep_ms(1000)
    if(L2.value()==0):  #y上
        i_y = i_y - 1
        Servo_y(S2,i_y*0.5)
        print(i_y)
        utime.sleep_ms(200)
    if(L3.value()==0):
        utime.sleep_ms(200)
    if(L4.value()==0):  #复位
        Servo_x(S1,-15)
        Servo_y(S2,-45)
        utime.sleep_ms(200)
        Servo_x(S1,0)
        Servo_y(S2,-34.5)
    #       Servo_x(S1,o_x)
    #      Servo_y(S2,o_y)
    #     print(o_x)
    #    print(o_y)
    #        i_x = o_x * 2
    #        i_y = o_y * 2
    #        Servo_x(S1,i_x)
    #        Servo_y(S2,i_y)
        print('复位')
        utime.sleep_ms(200)
    H1.value(1)
    H2.value(0)
    if(L1.value()==0):  #x左
        i_x = i_x + 1
        Servo_x(S1,0.5*i_x)
        print(i_x)
        utime.sleep_ms(200)
    if(L2.value()==0):  #y下
        i_y = i_y + 1
        Servo_y(S2,i_y*0.5)
        print(i_y)
        utime.sleep_ms(200)
    if(L3.value()==0):  #x右
        i_x = i_x - 1
        Servo_x(S1,0.5*i_x)
        print(i_x)
        utime.sleep_ms(200)

    if(L4.value()==0):
        Servo_x(S1,8.8)
        Servo_y(S2,-43.4)
        utime.sleep_ms(500)
        while iiii < 990:
            iiii = iiii + 1
            iiiii = 8.8 - 0.0192*iiii
            Servo_x(S1,iiiii)#-10.4
            utime.sleep_ms(2)
        iiii = 0
        while iiii < 1001:
            iiii = iiii + 1
            iiiii = -43.4 + 0.0169*iiii
            Servo_y(S2,iiiii)#-26.5
            utime.sleep_ms(2)
        iiii = 0
        while iiii < 980:
            iiii = iiii + 1
            iiiii = -10.4 + 0.0192*iiii
            Servo_x(S1,iiiii)#8.7
            utime.sleep_ms(2)
        iiii = 0
        while iiii < 1001:
            iiii = iiii + 1
            iiiii = -26.5 - 0.0169*iiii
            Servo_y(S2,iiiii)#-43.4
            utime.sleep_ms(2)
        iiii = 0
    H2.value(1)
